/*
 * IMU_math.cpp
 *
 *  Created on: Jan 19, 2014
 *      Author: ppks
 */

#include "Arduino.h"
#include "types.h"
#include "IMU.h"
#include "MathMethods.h"


/*******************************************************************************
* Function Name: IMU_rotateV32()
********************************************************************************
* Summary:
*  Rotate Estimated vector(s) with small angle approximation, according to the
*  gyro data
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void rotateV32( t_int32_t_vector *v,int16_t* delta)
{
    int16_t X = v->V16.X;
    int16_t Y = v->V16.Y;
    int16_t Z = v->V16.Z;

    v->V32.Z -=  mul(delta[ROLL]  ,  X)  + mul(delta[PITCH] , Y);
    v->V32.X +=  mul(delta[ROLL]  ,  Z)  - mul(delta[YAW]   , Y);
    v->V32.Y +=  mul(delta[PITCH] ,  Z)  + mul(delta[YAW]   , X);
}
